#ifndef P_INVERSE_H
#define P_INVERSE_H
#include "Eigen/SVD"
//-----------------------------------------------------------------------------
//! \brief pseudo_inverse
//! \brief Computes the pseudo-Inverse of a rectangular Matrix
//! \return true if the the SVD succeed
//! \param[in] The Original Matrix
//! \param[in] The Pseudo-Inverted Matrix
//! \tparam The type contained by the matrix (double, int...)
template<typename Scalar>
bool pinv(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &a, Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> &a_pinv)
{
    // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method

    if ( a.rows()<a.cols() )
        return false;

    // SVD
    Eigen::SVD< Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > svdA(a);

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> vSingular = svdA.singularValues();

    // Build a diagonal matrix with the Inverted Singular values
    // The pseudo inverted singular matrix is easy to compute :
    // is formed by replacing every nonzero entry by its reciprocal (inversing).
    Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Eigen::RowMajor> vPseudoInvertedSingular(svdA.matrixV().cols(),1);
        
    for (int iRow =0; iRow<vSingular.rows(); iRow++)
    {
        if ( fabs(vSingular(iRow))<=1e-10 ) // Todo : Put epsilon in parameter
        {
            vPseudoInvertedSingular(iRow,0)=0.;
        }
        else
        {
            vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
        }
    }

    // A little optimization here 
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> mAdjointU = svdA.matrixU().adjoint().block(0,0,vSingular.rows(),svdA.matrixU().adjoint().cols());

    // Pseudo-Inversion : V * S * U'
    a_pinv = (svdA.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU  ;

    return true;
};
//-----------------------------------------------------------------------------

#endif